#include "Mavlink.h"
#include "Markup.h"


MapMavlinkCfg CMavlink::mapMavlinkCfg;

CMavlink::CMavlink()
{
    fmt.load("xml/mavlink.xml");
    if(mapMavlinkCfg.size() == 0)
    {
        load("xml/config.xml");
        for (MapMavlinkCfg::iterator it = mapMavlinkCfg.begin(); it != mapMavlinkCfg.end(); it++)
        {
            it->second.fmt.load("xml/mavlink_" + it->second.xml + ".xml");
        }
    }
}

void CMavlink::load(const std::string &strXml)
{
    bool bFinished = 0;
    CMarkup xml;
    xml.Load(strXml.c_str());
    xml.ResetPos();

    if (!xml.FindElem())
        bFinished = 1;

    while (!bFinished)
    {
        // Process element
        std::string strTag = xml.GetTagName();
        if (strTag.compare("item") == 0)
        {
            MavlinkCfg item;
            int msgid = atoi(xml.GetAttrib("msgid").data());
            item.id = atoi(xml.GetAttrib("crcid").data());
            item.length = atoi(xml.GetAttrib("length").data());
            item.xml = xml.GetAttrib("xml");
            mapMavlinkCfg[msgid] = item;
        }

        // Next element (depth first)
        int bFound = xml.FindChildElem();
        while (!bFound && !bFinished)
        {
            if (xml.OutOfElem())
                bFound = xml.FindChildElem();
            else
                bFinished = 1;
        }
        if (bFound)
            xml.IntoElem();
    }
}

void CMavlink::print()
{
}

std::vector<unsigned char> CMavlink::encode(int msgid)
{
    std::vector<unsigned char> data;
    encode(msgid, data);
    return data;
}

int CMavlink::encode(int msgid, std::vector<unsigned char> &data)
{
    MapMavlinkCfg::iterator it = mapMavlinkCfg.find(msgid);
    if (it != mapMavlinkCfg.end())
    {
        int l = it->second.length + 10;
        it->second.fmt.encode(data);
        fmt.set("payload64", data);
        fmt.encode(data);
        data[l] = it->second.id;
        unsigned short crc = crc16_mcrf4xx(data.data() + 1, l);
        data[l] = (crc & 0xff);
        data[l + 1] = (crc >> 8);
        return l + 2;
    }
    return 0;
}

int CMavlink::decode(const std::vector<unsigned char> &d)
{
    return decode(d.data(), d.size());
}

int CMavlink::decode(const unsigned char *p, int nSize)
{
    fmt.decode(p, nSize);
    const std::vector<unsigned char> &v = boost::any_cast<std::vector<unsigned char>>(fmt.get("payload64"));
    int msgid = boost::any_cast<unsigned long long>(fmt.get("msgid"));
    MapMavlinkCfg::iterator it = mapMavlinkCfg.find(msgid);
    if (it != mapMavlinkCfg.end())
    {
        int l = it->second.length + 10;
        unsigned char c = p[l];
        ((unsigned char *)p)[l] = it->second.id;
        unsigned short crc = crc16_mcrf4xx(p + 1, l);
        if ((crc & 0xff) != c || (crc >> 8) != p[l + 1])
        {
            printf("%s crc error!\n", it->second.xml.c_str());
        }
        it->second.fmt.decode(v);
    }
    return msgid;
}

CFormat &CMavlink::get(int msgid)
{
    MapMavlinkCfg::iterator it = mapMavlinkCfg.find(msgid);
    if (it != mapMavlinkCfg.end())
    {     
        return it->second.fmt;
    }
    static CFormat fmt;
    return fmt;
}

unsigned short crc16_mcrf4xx(const unsigned char *data, size_t len)
{
    unsigned short crc = 0xFFFF;
    if (!data || len < 0)
        return crc;

    while (len--)
    {
        unsigned char tmp = *data++ ^ (crc & 0xFF);
        tmp ^= tmp << 4;
        crc = (crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4);
    }
    return crc;
}